#include "ros/ros.h"
#include "serial/serial.h"
#include "std_msgs/String.h"
#include <std_msgs/Float32.h>
#include <tcr100/ImuData.h>
#include <tcr100/serial.h>
#include <sensor_msgs/Imu.h>
#include "tf/transform_broadcaster.h"

serial::Serial ser; //声明串口对象
#define BUFFERSIZE 50
unsigned char buff[BUFFERSIZE],counter;
unsigned char sign=0;
float angle,acc_x,v_th;

unsigned char check_yaw(unsigned char *buffer)
{
  unsigned char ret,sum1,sum2,sum3;
for(int k=0;k<BUFFERSIZE;k++)
{
  if(buffer[k]==0x55&&buffer[k+1]==0x51)
  {
      sum1=buffer[k]+buffer[k+1]+buffer[k+2]+buffer[k+3]+buffer[k+4]+buffer[k+5]+buffer[k+6]+buffer[k+7]+buffer[k+8]+buffer[k+9];
      sum2=buffer[k+11]+buffer[k+12]+buffer[k+13]+buffer[k+14]+buffer[k+15]+buffer[k+16]+buffer[k+17]+buffer[k+18]+buffer[k+19]+buffer[k+20];
      sum3=buffer[k+22]+buffer[k+23]+buffer[k+24]+buffer[k+25]+buffer[k+26]+buffer[k+27]+buffer[k+28]+buffer[k+29]+buffer[k+30]+buffer[k+31];
      if((sum1==buffer[k+10])&&(sum2==buffer[k+21])&&(sum3==buffer[k+32]))//
      {
          ret=1;
          acc_x=(short(buffer[k+3]<<8| buffer[k+2]))/32768.0*16*9.8;//m/s2
          v_th=(short(buffer[k+18]<<8| buffer[k+17]))/32768.0*2000*0.017453292222;//rad/s
          angle=(short(buffer[k+29]<<8| buffer[k+28]))/32768.0*180;//degree
          printf("acc_x=:%f\n",acc_x);
          printf("v_th=:%f\n", v_th);
          printf("angle=:%f\n",angle);
      }
      else
      {
          ret=0;
      }
  }
}
  return ret;
}
int main (int argc, char** argv){
    ros::init(argc, argv, "serial_example_node");
    ros::NodeHandle nh;
    ros::Publisher yaw_pub = nh.advertise<tcr100::ImuData>("yaw_data", 1000);
    ros::Publisher imu_pub = nh.advertise<sensor_msgs::Imu>("imu", 1000);

    try
    {
        ser.setPort("/dev/ttyS1");
        ser.setBaudrate(9600);
        serial::Timeout to = serial::Timeout::simpleTimeout(1000);
        ser.setTimeout(to);
        ser.open();
    }
    catch (serial::IOException& e)
    {
        ROS_ERROR_STREAM("Unable to open port ");
        return -1;
    }
    if(ser.isOpen()){
        ROS_INFO_STREAM("Serial Port initialized");
    }else{
        return -1;
    }
    ros::Rate loop_rate(200);
    while(ros::ok()){
        tcr100::serial result;
        ros::spinOnce();

        if(ser.available()){
            ser.read(buff,BUFFERSIZE);
            if(check_yaw(buff)!=0)
            {
            }
            tcr100::ImuData msg;
            msg.acc_x=acc_x;
            msg.th_vel=v_th;
            msg.yaw=angle;
            yaw_pub.publish(msg);

            memset(buff,0,BUFFERSIZE);
        }
        loop_rate.sleep();
    }
}
